#include <functional>
#include <memory>
#include <thread>
#include <queue>
#include <sstream>

#include "rclcpp/rclcpp.hpp"
#include "test_interfaces/srv/test.hpp"

using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;

class TestClientWaitSet : public rclcpp::Node
{
private:
    int sum = 0;
    bool next_goal = true;
    std::queue<int> request_list;

    rclcpp::CallbackGroup::SharedPtr callback_group_service;
    rclcpp::Client<test_interfaces::srv::Test>::SharedPtr client_;
    rclcpp::WaitSet wait_set_;

public:
    TestClientWaitSet();
    ~TestClientWaitSet();
    void start();
    void send_request();
    void callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future);
};

TestClientWaitSet::TestClientWaitSet() : Node("test_client")
{
    client_ = this->create_client<test_interfaces::srv::Test>("add_three_ints");

    // 构造请求队列
    this->request_list.push(2);
    this->request_list.push(2);

    wait_set_.add_client(this->client_);
}

TestClientWaitSet::~TestClientWaitSet()
{
}

// 定时器回调
void TestClientWaitSet::start()
{
    while (rclcpp::ok() && !this->request_list.empty())
    {
        this->send_request();
        auto wait_result = wait_set_.wait(std::chrono::seconds(1));

        if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
        {
            if (wait_result.get_wait_set().get_rcl_wait_set().clients[0])
            {
                RCLCPP_INFO(this->get_logger(), "Wait-set handle client");
                test_interfaces::srv::Test::Response response;
                rmw_request_id_t request_id;
                if (this->client_->take_response(response, request_id))
                {
                    RCLCPP_INFO(this->get_logger(), "got response");
                    RCLCPP_INFO(this->get_logger(), "result is %d", response.sum);
                }

                this->request_list.pop();
            }
        }
        else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout)
        {
            RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout");
        }
    }

    return;
}

void TestClientWaitSet::send_request()
{
    // 暂停后续请求
    this->next_goal = false;

    // 发送此次请求
    auto request = std::make_shared<test_interfaces::srv::Test::Request>();
    request->a = 1;
    request->b = 2;
    request->c = 3;

    client_->async_send_request(request, std::bind(&TestClientWaitSet::callback, this, _1));
    RCLCPP_INFO(this->get_logger(), "sended request");
}

void TestClientWaitSet::callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future)
{
    // 处理请求结果
    auto result = future.get();
    this->sum = result->sum;
    RCLCPP_INFO(this->get_logger(), "got response");
    RCLCPP_INFO(this->get_logger(), "result is %d", this->sum);

    // 放开后续请求
    this->request_list.pop();
    this->next_goal = true;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto client = std::make_shared<TestClientWaitSet>();
    client->start();

    rclcpp::executors::MultiThreadedExecutor exe;
    exe.add_node(client);
    exe.spin();

    rclcpp::shutdown();
    return 0;
}